// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using base_interfaces_demo::action::Progress;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;
// 3.定义节点类；
class ActionClient : public rclcpp::Node
{
public:

  ActionClient(): Node("action_client_node_cpp")
  {
    // 3-1.创建动作客户端；
    _action_client = rclcpp_action::create_client<Progress>(this,"get_sum");
    RCLCPP_INFO(this->get_logger(), "动作客户端创建。");
  }

  // 3-2.发送请求；
  void send_goal(int64_t num)
  {
    while(!_action_client->wait_for_action_server(1s))
    {
      if (!rclcpp::ok()) 
      {
        RCLCPP_ERROR(this->get_logger(), "rclcpp异常！");
        exit(1);
      }
      else
      RCLCPP_INFO(this->get_logger(), "等待服务连接！");
    }
    auto goal = Progress::Goal();
    goal.num = num;
    RCLCPP_INFO(this->get_logger(), "发送请求数据！");

    auto send_goal_options = rclcpp_action::Client<Progress>::SendGoalOptions();
    send_goal_options.goal_response_callback =std::bind(&ActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =std::bind(&ActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =std::bind(&ActionClient::result_callback, this, _1);
    auto goal_handle_future = this->_action_client->async_send_goal(goal, send_goal_options);
  }

private:
  rclcpp_action::Client<Progress>::SharedPtr _action_client;

  // 3-3.处理目标发送后的反馈；
  void goal_response_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle)
  {
    if (!goal_handle) 
    {
      RCLCPP_ERROR(this->get_logger(), "目标请求被服务器拒绝！");
    } 
    else 
    {
      RCLCPP_INFO(this->get_logger(), "目标被接收，等待结果中");
    }
  }

  // 3-4.处理连续反馈；
  void feedback_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr,const std::shared_ptr<const Progress::Feedback> feedback)
  {
    int32_t progress = (int32_t)(feedback->progress * 100);
    RCLCPP_INFO(this->get_logger(), "当前进度: %d%%", progress);
  }

  // 3-5.处理最终响应。
  void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "任务被中止");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "任务被取消");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "未知异常");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "任务执行完毕，最终结果: %ld", result.result->sum);
  }
}; 

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc, argv);

  // 4.调用spin函数，并传入节点对象指针；
  auto action_client = std::make_shared<ActionClient>();
  action_client->send_goal(10);
  rclcpp::spin(action_client);
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}